#include "PhysicsPrimitives.h"
#include "Entity.h"

#include <cstdio>

void PhysicsPrimitive::AddLinearVelocity( const b2Vec2 & inVel ) {
	b2Vec2 vel = mBody->GetLinearVelocity();
	vel = vel + inVel;
	mBody->SetLinearVelocity(vel);
}

float PhysicsPrimitive::GetAngle(){
	return mBody->GetAngle();
}

void PhysicsPrimitive::SetLinearVelocity( const b2Vec2 & inVec ) {
	mBody->SetLinearVelocity(inVec);
}

void PhysicsPrimitive::AttachGameEntity( GameEntity * inEntity ){

	this->mGameEntity = inEntity;
}

b2Vec2 PhysicsPrimitive::GetLinearVelocity(  ) {
	return mBody->GetLinearVelocity();
}

b2Vec2 PhysicsPrimitive::GetPos() {
	return this->mBody->GetPosition();
}

//Box

PhysicsBox::PhysicsBox ( 
		const b2Vec2 & inTopRight, 
		const b2Vec2 & inTopLeft, 
		float thickness,
		const PhysicsProperties & inProps)	
{
	this->mProperties = inProps;
	b2Vec2 center;
	center.x = (inTopRight.x + inTopLeft.x)/2;
	center.y = (inTopRight.y + (inTopRight.y - thickness))/2;

	//XXX CLEAN THIOS UP ALOT.
	//It is confusing , because mOrigin must be set in physics properties,
	//the createBox function takes in absolute positions, and
	//this function has to make sure it inputs relative positions to the box's origin...
	b2Vec2 topRight = b2Vec2(inTopRight.x - center.x, inTopRight.y - center.y);
	b2Vec2 topLeft = b2Vec2(inTopLeft.x - center.x, inTopLeft.y - center.y);
	b2Vec2 bottomLeft = b2Vec2(topLeft.x, topLeft.y - thickness);
	b2Vec2 bottomRight = b2Vec2(topRight.x, topRight.y - thickness);
	
	b2Vec2 verts[4] = { topRight, topLeft,
		bottomLeft, bottomRight };

	mShape = new b2PolygonShape();
	b2PolygonShape * ps;
	ps = static_cast<b2PolygonShape *>(mShape);
	ps->Set(verts, 4);
}

PhysicsCircle::PhysicsCircle(
			float rad,
			const PhysicsProperties & inProps)
{
	this->mProperties = inProps;

	mShape = new b2CircleShape();
	b2CircleShape * cs = static_cast<b2CircleShape *>(mShape);
	cs->m_p.Set(0, 0);
	cs->m_radius = rad;
}

PhysicsChain::PhysicsChain( const b2Vec2 * pos, int numVerts, const PhysicsProperties & inProps )
{
	mProperties = inProps;

	b2ChainShape * cs = new b2ChainShape();
	cs->CreateChain(pos, numVerts);
	mShape = cs;
}